* 




\ Docket No. 2063.001700 
Client Docket No. VS-533 



Application for United States Letters Patent 



KALMAN FILTER WITH ADAPTIVE MEASUREMENT VARIANCE 

ESTIMATOR 



NUMBER ZL fM&i>&LlStl& 

DATE OF DEPOSIT ^MASMh£AJ% A*** 

I hereby certify that this paper or fee is being deposited with the United States 
Postal Service with sufficient postage "EXPRESS MAIL POST OFFICE TO 
ADDRESSEE" service under 37 C.F.R. 1.10 on the date indicated above and is 
addressed to: BOX PATENT APPLICATION, Assistant Commissioner for 
Patents, Washington, D.C. 20231. 



for 



by 



John I. Lipp, Ph.D. 



EXPRESS MAIL MAILING LABEL 





\ Docket No. 2063.001700 
Client Docket No. VS-533 



KALMAN FILTER WITH ADAPTIVE MEASUREMENT VARIANCE 



This application is related to my commonly assigned, co-pending application Serial 

No. / , filed on even date herewith, entitled "Multipath Mitigation Filter" (Attorney 

Docket No. 2063.001600; Client Docket No. VS-532), which is hereby incorporated by 
reference for all purposes as if expressly set forth verbatim herein. 

1. FIELD OF THE INVENTION 

The present invention pertains to the filtering of sensed signals and, in particular, to a 
Kalman filter including an adaptive measurement variance estimator. 

2. DESCRIPTION OF THE RELATED ART 

Discrete Kalman filtering is extensively used in modern day digital control and signal 
processing for a variety of purposes. Among these purposes are to: "smooth" signals, i.e., 
reduce measurement noise; observe unmeasured states; and/or predict future system states. 
In a typical discrete Kalman filter, all of the system parameters are specified. These system 
parameters typically include the: 

• state transition matrix; 

• state input gains; 

• plant uncertainty covariance function matrix; and 

• measurement covariance function matrix. 

However, the measurement covariance function matrix is frequently unknown. In these 
cases, an ad hoc value for the measurement covariance function matrix is used. Although 
this estimate is better than nothing, it nevertheless impairs the performance of the Kalman 
filter in its intended function, i.e., smoothing, observing, and/or predicting. 

For instance, consider a system described mathematically as follows using the 
notations defined in Table 1 below: 



ESTIMATOR 



BACKGROUND OF THE INVENTION 



x[n] = A[n]x[n - 1] + B[n]M{n] + C[n]u[n] 



(1) 



z[n] = H[n]x[n] + v[n] 
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(2) 

wherein: 

u[n] = the input to the system; 

z[n] = the measured output of the system; and 

x[n] = state of the system. 
The quantities w[n] and v[n] are mutually independent, white, noise processes with discrete 
variance functions 2[«]<5[«] and /?[/?]<5[/?], respectively. In the art, Eq. (1) is known as the 
"state transition model" and Eq. (2) is the "measurement model." One known filtering 
technique based on a Kalman filter employs the following set of equations for determining 

the measurement variance estimate R[n] for the «th sample: 

x[n | n - 1] = A[n]x[n - 1] + C[n] u[n] 



(3) 



P[n\n-l] = A[n]P[n - l]A[n] r + B[n]Q[n]B[nf 



(4) 

e[n] = z[n]-H[n]x[n\n-l] (5) 



[»] = *l [n - 1] + H[n]G[n](e 2 [«] - &] [n - 1]) 
R[n] = max(0, a] [n] - H[n]P[n \ n - \]H[n] T ) 
P[n\n-l]H T 



(6) 
(7) 



HP[n\n-\]H r +R[n] 
x[n] = x[n\n-\] + K[n](z[n] - H[n]x[n \n-l]) 
P[n) = (/ - K[n\H[n])P[n \n-\] 



(8) 
(9). 
(10) 

Note the calculation of the variance estimation R[ri\. Note also that these are but 
mathematical descriptions of sensed signals, e.g., electrical, magnetic, or optical signals 
representing tangible real world phenomena or characteristics of such phenomena. 

This technique can essentially be broken down into three parts. First, the current state 
is predicted (Eq. (3), Eq. (4)). Next, the variance of the measured quantity is estimated (Eq. 
(5), Eq. (6), and Eq. (7)). Then, the current state is updated with the measurement (Eq. (8), 

Eq. (9), Eq. (10)). This known technique's variance estimate R[n] is often quite large. Also, 
it is possible for the measurement variance estimate to be zero, which happens in practice, 
and causes all manner of problems. 
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Table 1. Definitions of Mathematical Notation 



Symbol 


Dimensions 


Descrintion 

m/fcj %*■ M M MJ *IV MM 


x[n] 


Nx 1 


System state vector (scalar in the case N = 1 ) 


x[n] 


Nx 1 


Current state vector estimate 


x\n\ n-U 


Nx 1 


Predicted state vector ( estimate of state vector from nreviouslv 
estimated state vector) 


P[n] 


NxN 


State vector estimate covariance matrix 


P[n\n-l] 


NxN 


State vector prediction covariance matrix 


w[n] 


Lx 1 


System driving noise 


QVri\ 


LxL 


System driving noise covariance matrix 


u[n] 


Jx 1 


System input vector 


z[n] 


1 x 1 


System measurement 


K[n] 


Nx 1 


Kalman filter gain vector 


v[n] 


1 x 1 


Measurement noise 


R[n] 


1 x 1 


Measurement variance 


R[n] 


1 x 1 


Estimated measurement variance 


A\ri\ 


NxN 


State transition matrix 


B[n] 


NxL 


System driving noise gain matrix 


C[«] 


Nx J 


System input gain matrix 


H[n] 


1 xN 


Input state gain vector 


G[n] 


lxl . 


Error filter gain 


m 


1 x 1 


Discrete kronecker delta function 


e[n] 


1 x 1 


Prediction error 




1 x 1 


Prediction Error variance estimate 



The present invention is directed to resolving, or at least reducing, one or all of the 
problems mentioned above. 

SUMMARY OF THE INVENTION 

The invention is a Kalman filter with an adaptive measurement variance estimator. 

In a first aspect, the invention includes a signal filtering mechanism, comprising a 
Kalman filter and a variance estimator. The Kalman filter is capable of receiving an input 
signal, a measured quantity signal, and a variance estimate, and outputting a state estimate 
signal. The variance estimator is capable of estimating the variance of the measured quantity 
signal and generating the variance estimate signal for use in filtering the input signal and the 
measured quantity signal, wherein estimating the variance of the measured quantity signal 
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includes determining a smoothed estimate of the measured quantity's variance from the 
measured quantity signal. 

In a second aspect, the invention includes a method for estimating the current state of 
a discrete, vector-state, scalar-measurement system. The method begins by estimating the 
variance of a measured quantity for use in filtering an input quantity and the measured 
quantity. The estimation includes determining a smoothed estimate of the instantaneous 
prediction error's variance. The method also filters the input quantity and the measured 
quantity through a Kalman filter using the estimated variance of the measured quantity signal. 

In other manifestations, the invention includes an electronic computing device 
programmed to perform such a method, a transmission medium over which such a method 
may be performed, or a program storage medium encoded with instruction that, when 
executed by a computer, perform a method for estimating the current state of a discrete, 
vector- state, scalar-measurement system. The method includes, inter alia, determining a 
smoothed estimate of the instantaneous prediction error's variance. 



The invention may be understood by reference to the following description taken in 
conjunction with the accompanying drawings, in which like reference numerals identify like 
elements, and in which: 

FIG. 1 conceptually depicts a signal filtering mechanism in accordance with the 
present invention; 

FIG. 2A and FIG. 2B conceptually depict particular embodiments of the variance 
estimator employed in the signal filtering mechanism of FIG. 1 in alternative embodiments; 

FIG. 3 is a flowchart of one iteration in a first embodiment of the filtering technique 
implemented by the filtering mechanism of FIG. 1; 

FIG. 4 illustrates a method for filtering a signal to estimate the variance of a 
measured quantity signal in accordance with the present invention; 

FIG. 5 depicts one particular electronic computing device with which the signal 
filtering mechanism of FIG. 1 and FIG. 4 may be implemented; 



BRIEF DESCRIPTION OF THE DRAWINGS 
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FIG- 6 conceptually illustrates one particular implementation of the filtering 
mechanism wherein the filtering mechanism is employed in an "observer" configuration; 

FIG. 7 is a flowchart of one iteration in a second embodiment of the filtering 
technique implemented by the filtering mechanism of FIG. 1; and 

FIG. 8 illustrates a method for estimating the current state of a discrete, vector-state, 
scalar-measurement system in accordance with the present invention. 

While the invention is susceptible to various modifications and alternative forms, 
specific embodiments thereof have been shown by way of example in the drawings and are 
herein described in detail. It should be understood, however, that the description herein of 
specific embodiments is not intended to limit the invention to the particular forms disclosed, 
but on the contrary, the intention is to cover all modifications, equivalents, and alternatives 
falling within the spirit and scope of the invention as defined by the appended claims. 



Illustrative embodiments of the invention are described below. In the interest of 
clarity, not all features of an actual implementation are described in this specification. It will 
of course be appreciated that in the development of any such actual embodiment, numerous 
implementation-specific decisions must be made to achieve the developers' specific goals, 
such as compliance with system-related and business-related constraints, which will vary 
from one implementation to another. Moreover, it will be appreciated that such a 
development effort, even if complex and time-consuming, would be a routine undertaking for 
those of ordinary skill in the art having the benefit of this disclosure. 

Turning now to the drawings, FIG. 1 conceptually illustrates a filtering mechanism 
100 in accordance with the present invention. The filtering mechanism 100 includes a 
Kalman filter 110 and a variance estimator 120. One particular implementation for the 
variance estimator 120 is conceptually illustrated in FIG. 2 A. A second, alternative 
particular implementation is shown in FIG. 2B. Note the time delay t in both FIG. 2A and 
FIG. 2B. Both these alternative embodiments are discussed further below. In accordance 
with the notation of Table 1, for the «th sampling of the associated quantities: 



DETAILED DESCRIPTION OF THE INVENTION 



u[n] = the input to the system; 
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z[n] = the measured output of the system; 

x[n] = the current state of the system; 

K[n] = the gain of the Kalman filter 110; and 

R [n] = the variance of the input, or the measured output, or both, depending on 
the embodiment. 



estimates of the variance of the input R[n] and the state of the system x[n], respectively. 

For a continuous-time system the typical notation is x(t). Here, the invention is in 
reference to a discrete-time system with a (generally) uniform sampling rate At, denoted x[ri], 
where n is an integer denoting the samples of x{t). That is, xfyi] = x(nAt) under ideal, uniform 
sampling. With this notation, a discrete, vector-state, scalar-measurement system model is 
denoted 



where w[n] and v[n] are mutually independent, white, noise processes with discrete-time 
variance functions £?["]8[/7] and i?[«]8[w], respectively. Eq. (11) is the state model and Eq. 
(12) is the measurement model for the system. 

Note that the present invention employs the same starting point as does the known 
filtering technique discussed above. The measurements z[ri] and input u[n] are Kalman 
filtered via the sequence of equations 



The "a" notation indicates an estimate for a given quantity. Thus, R[n] and x[n] are 



x[n] = A[n]x[n - 1] + B[n] + C[n]u[n] 



(11) 
(12) 



z[n] = H[n] x[n] + v[n] 



x[n | n - 1] = A[n]x[n - 1] + C[n] u[n] 



(13) 



P[n\n-\] = A[n]P[n - l]A[n] T + B[n]Q[n]B[n] T 



(14) 



K[n] = 



P[n\n-\]H[n] T 



H[n]P[n | n - l]H[n] T + R[n - 1] 



(15) 



x[n] = jc[«|/?-1] + K[n](z[n] - H[ri\x[n \n-X\) 



(16) 
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(17) 



to arrive at an estimate x[«]of the true state x[n], Eq. (13) is the prediction of x[n] and Eq. 
(14) is the associated covariance matrix P[ri\ prediction. Eqs. (15) - (17) update the state 
prediction x[n \ n - 1] and the predicted covariance matrix P[n \ n - 1] with the measurement 
z[n]. 

However, the variance estimation of the measured quantity signal z[n] is performed 
differently than in the known technique discussed above. The estimation of R[n] is 
accomplished by filtering the squared instantaneous prediction error e 2 [n] where 

e 2 [n] = (z[n]-H[n}x[n\n-l]f (Jg) 

vi'aEq. (19): 

["] = &\ \n - 1] + H[n] G[n{e 2 [n] - &) [n - 1]) { 

which gives a smoothed estimate of the error variance, cr e 2 [«] . A suitable choice, but not the 

only choice, for G[n] is K\n - 1]. This implementation uses a matrix of all ones, written [1], 
for G[l] since K[0] is undefined. Solve 

R[nf (la 2 [it] - 1)+ ^M^, 2 [«](l - 3a 2 [«])- 2g[»]]+ cr 2 + tr 2 [«]« 2 [»])= 0 

with the quadratic equation where 



(20) 



H[ n ]A[n]H[n] r 
H[n]H[n] T 

q[n] = (H[n]B[n])Q[n](H[n]B[n]y 



(21) 
(22) 



for the estimate R[n] of R[n\, which is used in the Kalman filter update cycle. Eqs. (18) thru 
(22) should be computed between the Kalman filter prediction and update; that is, between 
Eq. (14) and Eq. (15) as is shown in the flowchart of FIG. 3, discussed below. The value of 
g[n] in Eq. (20) is but one possible value for q[ri]. An order of magnitude increase or 
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decrease has been found to be appropriate depending on the implementation. Significant 
adjustment, i.e., more that a few percent, of a[n] is not recommended. 

The derivation of the determination for R [n] used above shall now be presented. 
Without loss of generality, the discrete-time index notation [ri] is suppressed except where 
explicitly required. It is Hx[n \ n - 1] that is interesting and its variance HP[n \ n - l]H T . As 
a Kalman filter converges, P[n] « P[n - 1] . Letting n — > oo, equality holds, that is, 
P[oo] = P[n] = P\n-Y\ . P[oo] can be found by solving the Ricatti equation. Likewise, as 
n oo, a] [oo] = var(e[oo]) = HP[oo]H T + R[co] . Assume that ^[oo] = R[oo] , that is, the 

estimator converges to the true value. From the Ricatti equation, 

1 1 1 

HP[^]H T " (HA)P[oo](HAf + {HB)Q{HB) T + [oo] 

For convenience, make the approximation 



{ha)p[oo](ha) t =(hah- 1 ]hp[oo]h 7 (hah- 1 ) t = 



k 2 



(24) 



where 1 =(hH t ) 1 H T is the Moores-Penrose inverse of H. The reason for performing 
this approximation is so that the substitution 

HP[cv]H r = a e 2 [oo]-J?[oo] * 

can be made to eliminate P[oo] . Combining (21) - (25) and performing some algebra yields 
R 2 [co](2a[n] 2 - 1)+ R[co][a 2 e [oo](l - 3a[*] 2 )- 2<jr[*]]+ cr 2 [ao]fe[n] + cr 2 [ooM*] 2 ) = 0 

(26) 

Typically, a Kalman filter converges, or "stabilizes," after a dozen or so measurements. After 
this point, it is also expected that the smoothed, squared, instantaneous error & 2 [n] has 

converged to approximately a] [oo] . This suggests using Eq. (26) at sample time n instead of 
oo which leads to the above invention. The approximation in Eq. (24) changes the left term of 
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the right-hand side of Eq. (23). Consequently, adjustment of q, generally upward, may 
improve performance. 



For a more concrete example of this first embodiment, consider a simple two- 
dimensional state system where x\[n] is position, x^fn] is velocity, and w[n] is acceleration. 
The position state x[n] is measured. The matrices of interest are then 



A[n] = 



1 At 
0 1 



B[n)Q[n]B[n] T =*l 

H[n] = [l 0] 



A/ 3 /3 At 2 /2 
At 2 /2 At 



(27) 

(28) 
(29) 



where At is the time step and cr 2 is the acceleration variance. Consequently, a[n] = 1 and 
q[n] = a 2 v At 3 /3 . Note, however, it has been experimentally found that q[n] = <x 2 A/ 
provides better results. Eq. (20) then becomes 

R[n] 2 - 2R[n][a 2 e [n] + q[n]]+ &] [n](q[n] + a] [«]) = 0 

which is easily solved with the quadratic equation for the positive root 



(30) 



R[n] = (q[n] + a] [«])+ [n](q[n] + &] [«]) 

This value is then used in the estimation, updates, and determinations discussed 
above. 



(31) 



Initial operation of the Kalman filter should not employ the solution of Eq. (20) until 
the filter has stabilized, or "converged." Up to that point, a fixed, reasonable, a priori 

estimate of the input measurement variance R[n] should be used in place of R[ri]. The 
Kalman filter should otherwise be initialized with standard methods such as a priori 
estimates x Q and P Q for initializing x[ri) and P[n], respectively. The conditions defining 
"convergence" or "stabilization" are well known to those skilled in the art. In one particular 
embodiment, the initialization period is chosen to be a number of iterations (e.g., 10-20) 
experimentally determined as a point after which the Kalman filter has converged. This 



Page 10 of 29 




A Docket No. 2063.001700 
Client Docket No. VS-533 



initialization takes approximately 10 seconds in this particular embodiment. However, the 
initialization period and its determination will be implementation specific. 

FIG. 3 is a flowchart of one iteration 300 of the first embodiment of the filtering 
technique discussed above. One such iteration 300 is performed for each measurement 
sample. The iteration 300 begins at the terminator 305 and determines in the decision block 
310 whether the Kalman filter needs to be initialized. If so, then the iteration 300 sets the 
value of x[n|/7-l] to x 0 , the value of .P[/?|/7-l] to Pq, and the value of G[n] to [1] in the block 
315. If not, then the embodiment applies Eq. (13) and Eq. (14) and sets the value of G[n] to 
£T[«-1] in the block 320. The iteration 300 then determines whether the Kalman filter has 
stabilized in the block 325. If so, then the iteration 300 applies Eq. (18), Eq. (19), Eq. (21), 
Eq. (22), and Eq. (20) in the block 330 to estimate the variance R [n] of the measured 
quantity z[ri\. If not, then the iteration 300 applies Eq. (18) and Eq. (19) and sets the value of 

R[n\Xo Ro in the block 335. The iteration 300 then performs the updates by applying the 
Kalman filter in Eq. (15), Eq. (16), and Eq. (17) in the block 340. The iteration 300 ends at 
the terminator 345. Subsequent iterations are performed until all the measurement samples n 
of interest have been filtered. 

Thus, one difference between the known technique discussed above and the present 
invention lies in the estimation R [n] of the variance of the measured quantity. FIG, 4 
illustrates one particular method 400 for estimating the variance of a measured quantity used 
to predict the current state of a discrete, vector-state, scalar-measurement system. The 
iteration 300 illustrated in FIG. 3 is but one particular embodiment of the method 400 in 

FIG. 4. The method 400 generally comprises estimating (finding R [n]) the variance R[n] of 
the measured quantity z[ri] for use in filtering an input quantity u[n] and the measured 
quantity z[n], as set forth in the box 405. Then, a smoothed estimate of an instantaneous 
prediction error's variance cr][ri\ is determined, as set forth in the box 410. Next, the input 

quantity u[n] and the measured quantity z[n] are filtered through a Kalman filter using the 
estimated input variance of the measured quantity signal, as set forth in the box 415. 
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However, in the filtering technique discussed above relative to FIG. 1, FIG- 2, and 
FIG. 3, the estimate of the input variance R [ri] is calculated very differently than in the 
known technique as was previously mentioned. This is apparent from comparing Eq. (10) 
with Eqs. (18) - (20). Thus, more particularly, the variance estimate R [n] in the present 
invention is arrived at by determining a smoothed estimate of the instantaneous error in the 
measured quantity z[/i], as set forth in the box 415 of FIG. 4. In the embodiment illustrated 
in FIG. 1, FIG. 2, and FIG. 3, determining the smoothed estimate includes first determining 
the squared instantaneous prediction error e 2 [n] and smoothing it to yield the smoothed, 

squared instantaneous prediction error a] [n] . The variance estimate R [n] is then estimated 
from the smoothed squared instantaneous prediction error cr] [n] . 

FIG. 5 illustrates one particular embodiment of a computing system 500 with which 
the invention may be implemented in one particular aspect of the invention. The present 
invention is typically implemented in software. A measured signal z[n\ is converted to a 
digital representation, and then filtered by processing the digital representation using software 
implementing the method described in relation to FIG. 4, or one of the variations thereon 
previously discussed, in the filtering mechanism 100 of FIG. 1. In various manifestations, 
the invention may also be: 

• an electronic computing device (e.g., the computer 505) programmed to 
perform the method 400 of FIG. 4; 

• a program storage medium (e.g., the floppy disk 510 or the optical disk 515) 
encoded with instructions that, when executed by a computer (e.g., the 
computer 505), perform the method 400 of FIG. 4; or 

a transmission medium (e.g., the cable 520) over which the method 400 of 
FIG. 4 may be performed on data stored on a remote computer (e.g., the 
computer 505). 

The invention admits wide variation in this aspect. For instance: 

• the illustrated computer 505 is a workstation, but may be a desktop, laptop, 
mini-, a mainframe computer, or a processor (e.g., a digital signal processor, 
or "DSP", or a microprocessor) embedded in a piece of equipment (e.g., a 
receiver) in various implementations; 
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the computing system 500 might comprise many computing devices such as 
the computer 505, configured in a local area network ("LAN"), a wide area 
network ("WAN"), a system area network ("SAN"), an "intranet," or even a 
portion of the Internet; 

the program storage medium may be magnetic (e.g., the floppy disk 510) or 
optical (e.g., the optical disk 515); 

the program storage medium may be random access (e.g., the floppy disk 510) 
or read only (e.g., the optical disk 515); and 

the illustrated transmission medium 520 is a coaxial cable, but may also be a 
twisted wire pair or an optical fiber. 



The invention is not limited to implementation specific details such as these. 

FIG. 6 conceptually illustrates one particular implementation 600 of the filtering 
mechanism 100 in FIG. 1 wherein the filtering mechanism is employed in an "observer" 
configuration using the filtering technique of the iteration 300 in FIG. 3. Since this 
implementation is an observer configuration, there is no input u[ri], i.e., u[n] = 0 and the 
Kalman filter 1 10 receives only the measured quantity z[ri]. Note that this does not affect the 
particular embodiment of the variance estimator 120 shown in FIG. 2, which does not receive 
the input u[n]. Thus, consider a discrete, vector-state, scalar-measurement system model 
defined as: 



wherein w[n] and v[n] are independent white Gaussian noises with covariance function 
matrices £?5[h] ^5[/i], respectively. The measurements z[ri] can be Kalman filtered via 
the sequence of equations similar to those set forth above. However, a prediction x[n | n — 1] 
for the current state x[ri] is determined using only the previous state vector value x[n - 1] 
(i.e., the state at the time of the previous sampling): 



x[n] = Ax[n - 1] + B\i{n] 



(32) 



z[n] = Hx[n] + v[ri\ 



(33) 



x[n\n-l] = Ax[n-l] 



(34) 
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Eq. (14) - Eq. (22) can then be applied in the manner discussed above for the general case. 
Again, in determining R[n] for the next Kalman filter cycle in the implementation of FIG. 6, 
it is Hx[n] that is interesting and its variance H P[ri\H T , and its derivation is similar to that 
in the general case, as well. Note that this particular embodiment uses the variance estimator 
of FIG. 2 A. 

In a second embodiment alternative to that discussed relative to FIG. 3, the estimated 
input variance is determined slightly differently. FIG. 7 is a flowchart illustrating one 
iteration of this second embodiment. The second embodiment replaces Eq. (18) with 

e 2 [n] = (z[n]-H[n)x[n]) 2 

The advantage is the error variance is lower after the filtering update. This changes the 
equation for R[n] to 

(36) 

which can be solved using the quartic or other polynomial root solving procedure. 

A special case of interest is when a = 1 . This reduces the quartic to a quadratic that is more 

easily solved yielding 



The disadvantage of this scheme is that the estimate R [«] of R[n] to the Kalman filter 
equations is delayed one iteration. Thus, Eq. (15) is replaced with 

P[n\n-l]H[n] T 



K[n] = 



H[n]P[n\n-l]H[n] T +R[n-\] 



and the variance estimation and Kalman filter "update" steps are temporally 
swapped. 
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For one implementation of this second embodiment, consider the special case of 
a = 1 . The required square root is typically an expensive operation. The square root can be 
eliminated by filtering the absolute error 

\e[r,]\ = \z[n]-H[n]x[n]\ 

via 

<?< W = <r. [n - 1] + H[ri] G[n](s[n]\e[n)\ -&.[rt-l]) 

instead of the squared error. The constant S[n] is a scale factor that accounts for the 
difference between using the absolute versus the squared error. For Gaussian noise, a typical 



model, S[n] = yjir/2 « 1 .25 . Then Eq. (37) becomes 



^[»] = (^[«]) 2 +CT e [«]V^T 



Typically, the square root of q[n] is a constant independent of n and only need be computed 
once, or is otherwise inexpensive to compute. This one implementation of the second 
embodiment follows the FIG. 7, wherein & 2 [n]-0 is changed to |<T e [«]| = 0, Eq. (35) is 

replaced by Eq. (39), Eq. (19) is replaced by Eq. (40), and Eq. (36) is replaced by Eq. (41), 
respectively. Note that this particular embodiment uses the variance estimator of FIG. 2B. 

The derivation of the determination for R used in this second embodiment shall now 
be presented. In this variation of the second embodiment, the alternate scheme the squared 

error is e 2 [n] = (z[n] — H[n] x[n]) 2 . The procedure is modified by eliminating P[qo] with the 

appropriate substitution found from the expected value of e 2 [n], 

HP[oo]H T -*[«,] 
ov fool 

' L J (42) 

yielding 

* 4 [°° ] / f v +^ 3 M^t4 + ^ 2 m( 2 ^ 2 -i{i-^—\+(R[^g + g 2 la 2 -\)=o 

(<7 e 2 [oo]) <U=o] \ cr e [co] J 
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which can be solved using a quartic or other root solving procedure. In the modified 
implementation of this embodiment, the constant S[n] in the modified alternate scheme is 
found via 



where E{} is the expectation operator. 

Returning to FIG. 7, the iteration 700 begins at the terminator 705 and first 
determines whether the Kalman filter 1 10 is initialized in the decision box 710. If so, then 
the iteration 700 sets x[n | n-1] = x 0 , P[n | n -1] = P 0 , and & 2 e [ri\ = 0, as set forth in the box 
715. If not, then the iteration 700 applies Eq. (13) and Eq. (14) in box 720 to predict the state 
estimate x[n \ n-1] . The iteration 700 then applies Eq. (38), Eq. (16), and Eq. (17), as set 
forth in the box 725. The iteration 700 then determines whether the Kalman filter 110 is 
stable in the decision box 730. If so, the iteration 700 applies Eq. (35), Eq. (19), Eq. (21), Eq. 
(22), and Eq. (36), as set forth in the box 735 to estimate the variance of the measured 
quantity z[ri\. If not, the iteration 700 applies Eq. (35) and Eq. (19) and sets R[n] = R 0 , as set 
forth in the box 740. The iteration 700 then ends at the terminator 750. Subsequent iterations 
are performed until all the samples n of interest have been filtered. 

Now, extrapolating from the first and second embodiments illustrated in FIG. 3 and 
in FIG. 7, FIG. 8 illustrates a method 800 for estimating the current state of a discrete, 
vector-state, scalar-measurement system in accordance with the present invention. Assuming 
the system is initialized and that the Kalman filter has stabilized, the method 800 begins by 
first determining a current state vector prediction x[n \ n - 1] from a previous state vector 
estimate x[n - 1] and an input vector u[n], as set forth in the box 805. Next, the method 800 
determines a current state vector prediction covariance matrix P[n\n -1] associated with the 
current state vector prediction x[n \ n - 1] from a previous state vector covariance matrix 
P[n-l] associated with the previous state vector estimate x[n - 1] , as set forth in the box 810. 

The variance estimate R [n] for the measured quantity z[n] is then determined, as set 
forth in the box 815. This estimation is performed using the method 400 in FIG. 4, e.g., as in 




(44) 
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one of the first and second embodiments illustrated in FIG. 3 and in FIG. 7. The current 
Kalman filter gain vector K[n] is then determined from the current state vector prediction 
co variance matrix P[n \ n -1] and the measured quantity variance estimate, as set forth in the 
box 820. Next, the current state vector estimate is determined from the Kalman filter gain 
K[ri\, the current state vector prediction, and the measured quantity, as set forth in the box 
825. The current state vector covariance matrix associated with the current state vector 
estimate is then determined from the Kalman filter gain and the current state vector prediction 
covariance matrix, as set forth in the box 830. Finally, the method 800 iterates the above, as 
set forth in the box 835, until all samples of the system have been filtered. 

This concludes the detailed description. Note that some portions of the detailed 
descriptions herein are presented in terms of a software implemented process involving 
symbolic representations of operations on data bits within a memory in a computing system 
or a computing device. These descriptions and representations are the means used by those in 
the art to most effectively convey the substance of their work to others skilled in the art. The 
process and operation require physical manipulations of physical quantities. Usually, though 
not necessarily, these quantities take the form of electrical, magnetic, or optical signals 
capable of being stored, transferred, combined, compared, and otherwise manipulated. It has 
proven convenient at times, principally for reasons of common usage, to refer to these signals 
as bits, values, elements, symbols, characters, terms, numbers, or the like. 

It should be borne in mind, however, that all of these and similar terms are to be 
associated with the appropriate physical quantities and are merely convenient labels applied 
to these quantifies. Unless specifically stated or otherwise as may be apparent, throughout 
the present disclosure, these descriptions refer to the action and processes of an electronic 
device, that manipulates and transforms data represented as physical (electronic, magnetic, or 
optical) quantities within some electronic device's storage into other data similarly 
represented as physical quantities within the storage, or in transmission or display devices. 
Exemplary of the terms denoting such a description are, without limitation, the terms 
"processing," "computing," "calculating," "determining," "displaying," and the like. 

Furthermore, the invention has numerous applications in a wide variety of fields. One 
such application is disclosed in my commonly assigned, co-pending application Serial No. 
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# 
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/ , filed on even date herewith, entitled "Multipath Mitigation Filter" (Attorney 

Docket No. 2063.001600; Client Docket No. VS-532), incorporated by reference above. 
Multipath is a condition wherein received electromagnetic radiation may have propagated 
over multiple paths, which introduces error or uncertainty in measurements derived from 
those signals. For instance, a radar signal received after being echoed from an object may 
have traveled directly and/or indirectly from the object to the receiver. The present invention 
can be used to mitigate uncertainty or error arising from this phenomenon. Still other 
applications may become apparent to those skilled in the art having the benefit of this 
disclosure. For example, the invention may also be used in an image processing application. 
In this case, x- and y- coordinates are used instead of time and spatial invariance is used 
instead of time-invariance. 

Finally, the particular embodiments disclosed above are illustrative only, as the 
invention may be modified and practiced in different but equivalent manners apparent to 
those skilled in the art having the benefit of the teachings herein. Furthermore, no limitations 
are intended to the details of construction or design herein shown, other than as described in 
the claims below. It is therefore evident that the particular embodiments disclosed above 
may be altered or modified and all such variations are considered within the scope and spirit 
of the invention. Accordingly, the protection sought herein is as set forth in the claims 
below. 
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